﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CNEMC_WPF
{
    public static class Fitting
    {
        //网络搜索：最小二乘法曲面拟合
        //https://blog.csdn.net/Haipai1998/article/details/85345823
        public static void Calc(IEnumerable<FittingPoint> points, int meshgrids, out double[,] data)
        {
            //输出数据的实际坐标不重要，因为图像没有数轴，且空间位置由KML文件定义
            double
                sigma_x = 0, sigma_y = 0, sigma_z = 0,
                sigma_x2 = 0, sigma_y2 = 0,
                sigma_x3 = 0, sigma_y3 = 0,
                sigma_x4 = 0, sigma_y4 = 0,
                sigma_x_y = 0,
                sigma_x_y2 = 0,
                sigma_x_y3 = 0,
                sigma_x2_y = 0,
                sigma_x2_y2 = 0,
                sigma_x3_y = 0,
                sigma_z_x2 = 0,
                sigma_z_y2 = 0,
                sigma_z_x_y = 0,
                sigma_z_x = 0,
                sigma_z_y = 0;
            //以下循环耗时较久，根据数据量约1.5分钟（i5-9）
            for (int i = 0; i < points.Count(); i++)
            {
                var p = points.ElementAt(i);
                sigma_x += p.X;
                sigma_x2 += p.X * p.X;
                sigma_x3 += p.X * p.X * p.X;
                sigma_x4 += p.X * p.X * p.X * p.X;
                sigma_y += p.Y;
                sigma_y2 += p.Y * p.Y;
                sigma_y3 += p.Y * p.Y * p.Y;
                sigma_y4 += p.Y * p.Y * p.Y * p.Y;
                sigma_z += p.Z;
                sigma_x_y += p.X * p.Y;
                sigma_x_y2 += p.X * p.Y * p.Y;
                sigma_x_y3 += p.X * p.Y * p.Y * p.Y;
                sigma_x2_y += p.X * p.X * p.Y;
                sigma_x2_y2 += p.X * p.X * p.Y * p.Y;
                sigma_x3_y += p.X * p.X * p.X * p.Y;
                sigma_z_x2 += p.Z * p.X * p.X;
                sigma_z_y2 += p.Z * p.Y * p.Y;
                sigma_z_x_y += p.Z * p.X * p.Y;
                sigma_z_x += p.Z * p.X;
                sigma_z_y += p.Z * p.Y;
            }
            double[,] a = {{sigma_x4, sigma_x3_y, sigma_x2_y2, sigma_x3, sigma_x2_y, sigma_x2},
               {sigma_x3_y, sigma_x2_y2, sigma_x_y3, sigma_x2_y, sigma_x_y2, sigma_x_y},
               {sigma_x2_y2, sigma_x_y3, sigma_y4, sigma_x_y2, sigma_y3, sigma_y2},
               {sigma_x3, sigma_x2_y, sigma_x_y2, sigma_x2, sigma_x_y, sigma_x},
               {sigma_x2_y, sigma_x_y2, sigma_y3, sigma_x_y, sigma_y2, sigma_y},
               {sigma_x2, sigma_x_y, sigma_y2, sigma_x, sigma_y, points.Count()}};
            double[] b = { sigma_z_x2, sigma_z_x_y, sigma_z_y2, sigma_z_x, sigma_z_y, sigma_z };

            //高斯消元解线性方程
            Heroius.XuAlgrithms.LinearEquations.GAUS(a, b);
            data = new double[meshgrids, meshgrids];
            double x, y,
                x_step = (points.Last().X - points.First().X) / (meshgrids - 1),
                y_step = (points.Last().Y - points.First().Y) / (meshgrids - 1);

            x = points.First().X;
            for (int i = 0; i < meshgrids; i++)
            {
                y = points.First().Y;
                for (int j = 0; j < meshgrids; j++)
                {
                    data[i, j] = b[0] * x * x + b[1] * x * y + b[2] * y * y + b[3] * x + b[4] * y + b[5];
                    y += y_step;
                }
                x += x_step;
            }
        }
    }

    public class FittingPoint
    {
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
    }
}
